#include <noether_tpp/tool_path_planners/raster/plane_slicer_raster_planner.h>

#include <algorithm>  // std::find(), std::reverse(), std::unique()
#include <numeric>    // std::iota()
#include <stdexcept>  // std::runtime_error
#include <string>     // std::to_string()
#include <utility>    // std::move()
#include <vector>     // std::vector

#include <pcl/common/common.h>  // pcl::getMinMax3d()
#include <pcl/common/pca.h>     // pcl::PCA
#include <pcl/conversions.h>    // pcl::fromPCLPointCloud2
#include <pcl/surface/vtk_smoothing/vtk_utils.h>
#include <vtkAppendPolyData.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkCellLocator.h>
#include <vtkCutter.h>
#include <vtkDoubleArray.h>
#include <vtkErrorCode.h>
#include <vtkKdTreePointLocator.h>
#include <vtkParametricSpline.h>
#include <vtkPlane.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyDataNormals.h>
#include <vtkSmartPointer.h>
#include <vtkStripper.h>

namespace
{
static const double EPSILON = 1e-6;

// 定义一个结构体 RasterConstructData，用于存储栅格数据相关的信息
struct RasterConstructData
{
  // 一个存储栅格段的多态数据容器，每个栅格段是 vtkPolyData 类型
  std::vector<vtkSmartPointer<vtkPolyData>> raster_segments;

  // 一个存储每个栅格段长度的数组
  std::vector<double> segment_lengths;
};

// 计算旋转矩阵，将三个输入的 3D 向量（vx, vy, vz）作为基向量生成旋转矩阵
Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz)
{
  // 创建一个 3x3 的旋转矩阵，并初始化为单位矩阵
  Eigen::Matrix3d m;
  m.setIdentity();
  
  // 将输入的 vx, vy, vz 向量分别归一化并赋值给旋转矩阵的列
  // 这意味着 vx, vy, vz 是旋转矩阵的列向量
  m.col(0) = vx.normalized();  // 第一列为 vx 向量的单位化
  m.col(1) = vy.normalized();  // 第二列为 vy 向量的单位化
  m.col(2) = vz.normalized();  // 第三列为 vz 向量的单位化

  // 返回计算得到的旋转矩阵
  return m;
}

// 计算给定点集的总长度，假设点集按顺序连接成一条折线
double computeLength(const vtkSmartPointer<vtkPoints>& points)
{
  // 获取点集中的点数
  const vtkIdType num_points = points->GetNumberOfPoints();

  // 初始化总长度为 0
  double total_length = 0.0;

  // 如果点数小于 2，则无法计算长度，直接返回 0
  if (num_points < 2)
  {
    return total_length;
  }

  // 定义两个 3D 向量，用于存储相邻两个点的坐标
  Eigen::Vector3d p0, pf;

  // 遍历点集，计算每一对相邻点之间的距离并累加到 total_length 中
  for (vtkIdType i = 1; i < num_points; i++)
  {
    // 获取相邻的两个点的坐标，分别赋值给 p0 和 pf
    points->GetPoint(i - 1, p0.data());  // 获取第 i-1 个点
    points->GetPoint(i, pf.data());      // 获取第 i 个点

    // 计算这两个点之间的距离，并将其累加到 total_length
    total_length += (pf - p0).norm();
  }

  // 返回计算出的总长度
  return total_length;
}

// 通过样条曲线对原始点集进行平滑处理，并按照指定的点间距生成新的点集，从而实现了曲线上的均匀分布点生成。
vtkSmartPointer<vtkPoints> applyParametricSpline(const vtkSmartPointer<vtkPoints>& points,
                                                 double total_length,
                                                 double point_spacing)
{
  // 创建一个新的 vtkPoints 对象，用来存储新的点
  vtkSmartPointer<vtkPoints> new_points = vtkSmartPointer<vtkPoints>::New();

  // 创建一个 vtkParametricSpline 对象，用来生成样条曲线
  vtkSmartPointer<vtkParametricSpline> spline = vtkSmartPointer<vtkParametricSpline>::New();
  
  // 设置样条曲线的输入点集
  spline->SetPoints(points);

  // 通过长度对样条进行参数化，确保样条根据路径长度均匀分布
  spline->SetParameterizeByLength(true);

  // 将样条设置为开放曲线（非闭合曲线）
  spline->ClosedOff();

  // 初始化前一个点的坐标
  Eigen::Vector3d pt_prev;
  // 获取输入点集的第一个点，并存储到 pt_prev 中
  points->GetPoint(0, pt_prev.data());

  // 将第一个点插入到新的点集 new_points 中
  new_points->InsertNextPoint(pt_prev.data());

  // 计算需要生成的总点数，按给定的点间距进行分布
  std::size_t num_points = static_cast<std::size_t>(std::ceil(total_length / point_spacing) + 1);
  
  // 临时数组，用来存储样条计算的导数值
  double du[9];
  // Eigen 矢量 u 用来表示参数值，pt 用来存储样条计算得到的点坐标
  Eigen::Vector3d u, pt;

  // 从第二个点开始，计算剩余的点
  for (unsigned i = 1; i < num_points; i++)
  {
    // 计算当前点对应的参数值（归一化到 [0, 1] 区间）
    double interv = static_cast<double>(i) / static_cast<double>(num_points - 1);

    // 确保参数值不超过 1.0
    interv = interv > 1.0 ? 1.0 : interv;

    // 如果已经接近曲线的末尾，停止计算
    if (std::abs(interv - 1.0) < EPSILON)
    {
      break;  // 到达曲线的末尾，停止插入新的点
    }

    // 使用 Eigen::Vector3d::Ones() 来创建一个 3D 向量，其中每个分量都初始化为 interv
    u = interv * Eigen::Vector3d::Ones();

    // 使用 std::tie 为向量 u 分配相同的值
    std::tie(u[0], u[1], u[2]) = std::make_tuple(interv, interv, interv);

    // 计算样条在参数 u 处的值，并将结果存储到 pt 中
    spline->Evaluate(u.data(), pt.data(), du);

    // 检查计算出的点与前一个点之间的距离
    if ((pt - pt_prev).norm() >= point_spacing)
    {
      // 如果距离大于或等于设定的点间距，则将新的点添加到 new_points 中
      new_points->InsertNextPoint(pt.data());
      // 更新前一个点为当前点
      pt_prev = pt;
    }
  }

  // 将输入点集的最后一个点添加到新的点集中
  points->GetPoint(points->GetNumberOfPoints() - 1, pt_prev.data());
  new_points->InsertNextPoint(pt_prev.data());

  // 返回生成的新的点集
  return new_points;
}

/**
 * @brief removes points that appear in multiple lists such that only one instance of that point
 *        index remains
 * @param points_lists
 */
// 该函数的目的是从多个点列表中去除重复的点，使得每个点索引只保留一次。每个点索引只会出现在一个列表中。
void removeRedundant(std::vector<std::vector<vtkIdType>>& points_lists)
{
  // 定义IdList为vtkIdType类型的vector（即存储点的索引）
  using IdList = std::vector<vtkIdType>;

  // 如果points_lists的大小小于2，则无需执行任何操作，直接返回
  if (points_lists.size() < 2)
  {
    return;
  }

  // 定义存储去重后的点列表的容器
  std::vector<std::vector<vtkIdType>> new_points_lists;

  // 将points_lists中的第一个列表添加到new_points_lists中
  new_points_lists.push_back(points_lists.front());

  // 遍历points_lists中的其他列表，从第二个列表开始处理
  for (std::size_t i = 1; i < points_lists.size(); i++)
  {
    // 获取当前处理的点列表
    IdList& current_list = points_lists[i];
    // 定义新列表用于存储不重复的点
    IdList new_list;
    // 定义all_ids用于存储之前所有列表中的点索引
    IdList all_ids;

    // 遍历new_points_lists中的所有列表，将所有点的索引添加到all_ids中
    for (auto& ref_list : new_points_lists)
    {
      // 将ref_list中的所有点索引加入all_ids
      all_ids.insert(all_ids.end(), ref_list.begin(), ref_list.end());
    }

    // 遍历当前点列表中的每一个点索引
    for (auto& id : current_list)
    {
      // 如果id没有出现在all_ids中，则将其添加到新列表new_list中
      if (std::find(all_ids.begin(), all_ids.end(), id) == all_ids.end())
      {
        new_list.push_back(id);  // 添加当前点到new_list
      }
    }

    // 如果新列表new_list有点（即去重后有剩余点），则将其加入new_points_lists
    if (new_list.size() > 0)
    {
      new_points_lists.push_back(new_list);
    }
  }

  // 清空原始的points_lists
  points_lists.clear();

  // 将去重后的点列表（new_points_lists）复制回points_lists
  points_lists.assign(new_points_lists.begin(), new_points_lists.end());
}


// 是通过检查相邻点列表之间的距离，按照设定的合并阈值 merge_dist 将多个点列表合并为一个
void mergeRasterSegments(const vtkSmartPointer<vtkPoints>& points,
                         double merge_dist,
                         std::vector<std::vector<vtkIdType>>& points_lists)
{
  // 使用Eigen库来简化数学运算（主要用于处理3D点的向量运算）
  using namespace Eigen;
  
  // 定义IdList为vtkIdType类型的vector（即存储点的索引）
  using IdList = std::vector<vtkIdType>;

  // 如果points_lists的大小小于2，直接返回，无需合并
  if (points_lists.size() < 2)
  {
    return;
  }

  // 定义存储新的点列表的容器
  std::vector<IdList> new_points_lists;
  
  // 定义合并后的点的id和实际合并的点的列表
  IdList merged_list_ids;
  IdList merged_list;

  // do_merge lambda函数：用于检查两个点列表是否满足合并条件
  // 如果两个点的距离小于merge_dist，则将它们合并为一个列表
  auto do_merge =
      [&points](const IdList& current_list, const IdList& next_list, double merge_dist, IdList& merged_list) {
        // 使用Eigen库的Vector3d来存储3D点
        Vector3d cl_point, nl_point;

        // 检查current_list的前端与next_list的后端的距离
        points->GetPoint(current_list.front(), cl_point.data());
        points->GetPoint(next_list.back(), nl_point.data());
        double d = (cl_point - nl_point).norm();  // 计算两点之间的欧几里得距离
        if (d < merge_dist)
        {
          // 如果满足合并条件，先将next_list的点加入merged_list，再将current_list的点加入
          merged_list.assign(next_list.begin(), next_list.end());
          merged_list.insert(merged_list.end(), current_list.begin(), current_list.end());
          return true;  // 成功合并
        }

        // 检查current_list的后端与next_list的前端的距离
        points->GetPoint(current_list.back(), cl_point.data());
        points->GetPoint(next_list.front(), nl_point.data());
        d = (cl_point - nl_point).norm();  // 计算两点之间的欧几里得距离
        if (d < merge_dist)
        {
          // 如果满足合并条件，先将current_list的点加入merged_list，再将next_list的点加入
          merged_list.assign(current_list.begin(), current_list.end());
          merged_list.insert(merged_list.end(), next_list.begin(), next_list.end());
          return true;  // 成功合并
        }
        return false;  // 如果都不满足合并条件，返回false
      };

  // 遍历points_lists中的每个点列表，进行合并操作
  for (std::size_t i = 0; i < points_lists.size(); i++)
  {
    // 如果当前点列表已经被合并，跳过
    if (std::find(merged_list_ids.begin(), merged_list_ids.end(), i) != merged_list_ids.end())
    {
      continue;
    }

    // 获取当前点列表
    IdList current_list = points_lists[i];
    Vector3d cl_point, nl_point;

    // 记录是否需要继续查找相邻的列表进行合并
    bool seek_adjacent = true;
    
    // 在当前列表与相邻的点列表之间进行合并操作，直到没有更多合并的情况
    while (seek_adjacent)
    {
      seek_adjacent = false;  // 默认不再查找相邻点列表

      // 遍历points_lists中的剩余点列表，与当前列表尝试合并
      for (std::size_t j = i + 1; j < points_lists.size(); j++)
      {
        // 如果当前点列表已经合并，跳过
        if (std::find(merged_list_ids.begin(), merged_list_ids.end(), j) != merged_list_ids.end())
        {
          continue;
        }

        // 清空merged_list以准备存储合并后的结果
        merged_list.clear();
        IdList next_list = points_lists[j];

        // 尝试正向合并（当前列表的前端与下一个列表的后端）
        if (do_merge(current_list, next_list, merge_dist, merged_list))
        {
          current_list = merged_list;  // 合并成功，更新current_list
          merged_list_ids.push_back(static_cast<vtkIdType>(j));  // 标记该列表已经合并
          seek_adjacent = true;  // 继续查找与合并后的列表相邻的点列表
          continue;  // 跳到下一次while循环
        }

        // 尝试反向合并（当前列表的后端与下一个列表的前端）
        std::reverse(next_list.begin(), next_list.end());
        if (do_merge(current_list, next_list, merge_dist, merged_list))
        {
          current_list = merged_list;  // 合并成功，更新current_list
          merged_list_ids.push_back(static_cast<vtkIdType>(j));  // 标记该列表已经合并
          seek_adjacent = true;  // 继续查找与合并后的列表相邻的点列表
          continue;  // 跳到下一次while循环
        }
      }
    }
    
    // 将合并后的列表添加到新的点列表中
    new_points_lists.push_back(current_list);
  }

  // 清空原有的points_lists，并将合并后的点列表赋值给它
  points_lists.clear();
  
  // 将new_points_lists中点数大于1的列表复制回points_lists
  std::copy_if(new_points_lists.begin(), new_points_lists.end(), std::back_inserter(points_lists), [](const IdList& l) {
    return l.size() > 1;  // 只保留点数大于1的列表
  });
}

void rectifyDirection(const vtkSmartPointer<vtkPoints>& points,
                      const Eigen::Vector3d& ref_point,
                      std::vector<std::vector<vtkIdType>>& points_lists)
{
  // 定义一个 Eigen::Vector3d 类型的变量 p0 和 pf 用于存储第一和最后一个点的坐标
  Eigen::Vector3d p0, pf;

  // 如果 points_lists 的大小小于 2，表示没有足够的点进行操作，直接返回
  if (points_lists.size() < 2)
    return;

  // 获取 points_lists 中第一个列表中的第一个点的坐标，存入 p0
  points->GetPoint(points_lists.front().front(), p0.data());

  // 获取 points_lists 中最后一个列表中的最后一个点的坐标，存入 pf
  points->GetPoint(points_lists.back().back(), pf.data());

  // 计算 ref_point 到 p0 和 pf 的距离，并根据距离比较决定是否需要反转
  // 如果 ref_point 到 p0 的距离大于到 pf 的距离，设置 reverse 为 true
  bool reverse = (ref_point - p0).norm() > (ref_point - pf).norm();

  // 如果 reverse 为 true，表示需要反转方向
  if (reverse)
  {
    // 遍历 points_lists 中的每个列表，对每个列表中的元素进行反转
    for (auto& s : points_lists)
    {
      std::reverse(s.begin(), s.end());
    }

    // 对 points_lists 进行反转，颠倒列表的顺序
    std::reverse(points_lists.begin(), points_lists.end());
  }
}

noether::ToolPaths convertToPoses(const std::vector<RasterConstructData>& rasters_data)
{
  // 创建一个空的 ToolPaths 容器，用于存储最终的路径数据
  noether::ToolPaths rasters_array;
  
  // 定义反转标志，控制路径方向
  bool reverse = true;
  
  // 遍历每个 RasterConstructData 数据
  for (const RasterConstructData& rd : rasters_data)
  {
    // 每次迭代反转路径方向
    reverse = !reverse;
    
    // 创建一个新的 ToolPath，用于存储当前的路径数据
    noether::ToolPath raster_path;
    
    // 将当前 RasterConstructData 中的 raster_segments 拷贝到一个新的 vector 中
    std::vector<vtkSmartPointer<vtkPolyData>> raster_segments;
    raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end());
    
    // 如果 reverse 为 true，则反转 raster_segments 的顺序
    if (reverse)
    {
      std::reverse(raster_segments.begin(), raster_segments.end());
    }

    // 遍历每个 raster_segment，处理每个 segment 中的点数据
    for (const vtkSmartPointer<vtkPolyData>& polydata : raster_segments)
    {
      // 创建一个新的 ToolPathSegment 用于存储路径段
      noether::ToolPathSegment raster_path_segment;
      
      // 获取当前 polydata 中点的数量
      std::size_t num_points = polydata->GetNumberOfPoints();
      
      // 定义一些用于计算的向量
      Eigen::Vector3d p, p_next, vx, vy, vz;
      Eigen::Isometry3d pose;  // 定义一个位姿，用于存储计算出的位姿数据
      
      // 创建一个索引列表，并填充从 0 到 num_points - 1 的整数
      std::vector<int> indices(num_points);
      std::iota(indices.begin(), indices.end(), 0);
      
      // 如果 reverse 为 true，反转索引的顺序
      if (reverse)
      {
        std::reverse(indices.begin(), indices.end());
      }

      // 遍历所有点并计算每两个相邻点之间的位姿
      for (std::size_t i = 0; i < indices.size() - 1; i++)
      {
        int idx = indices[i];        // 当前点的索引
        int idx_next = indices[i + 1];  // 下一个点的索引
        
        // 获取当前点和下一个点的坐标
        polydata->GetPoint(idx, p.data());
        polydata->GetPoint(idx_next, p_next.data());
        
        // 获取当前点的法线向量
        polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data());
        
        // 计算两点间的方向向量 vx（即从 p 到 p_next）
        vx = (p_next - p).normalized();
        
        // 计算正交方向向量 vy（与 vx 和 vz 的叉积）
        vy = vz.cross(vx).normalized();
        
        // 再次计算 vx，以确保其正交性
        vx = vy.cross(vz).normalized();
        
        // 计算从原点到 p 的平移以及由 vx, vy, vz 计算的旋转
        pose = Eigen::Translation3d(p) * Eigen::AngleAxisd(computeRotation(vx, vy, vz));
        
        // 将计算出的位姿添加到 raster_path_segment 中
        raster_path_segment.push_back(pose);
      }

      // 添加最后一个位姿，位置为 p_next，方向与前一个位姿相同
      pose.translation() = p_next;  // 位置更新为 p_next，方向保持不变
      raster_path_segment.push_back(pose);

      // 将当前的 raster_path_segment 添加到 raster_path 中
      raster_path.push_back(raster_path_segment);
    }

    // 如果 raster_path 非空，将其添加到最终的 rasters_array 中
    if (!raster_path.empty())
      rasters_array.push_back(raster_path);
  }

  // 返回最终构造的路径数据
  return rasters_array;
}


bool insertNormals(const double search_radius,
                   vtkSmartPointer<vtkPolyData>& mesh_data_,
                   vtkSmartPointer<vtkKdTreePointLocator>& kd_tree_,
                   vtkSmartPointer<vtkPolyData>& data)
{
  // 创建一个新的 vtkDoubleArray，用来存储新的法线数据
  vtkSmartPointer<vtkDoubleArray> new_normals = vtkSmartPointer<vtkDoubleArray>::New();
  new_normals->SetNumberOfComponents(3);  // 每个法线包含三个分量 (x, y, z)
  new_normals->SetNumberOfTuples(data->GetPoints()->GetNumberOfPoints());  // 设置法线数据的大小为输入数据中的点数

  // 获取原始网格数据（mesh_data_）中的法线信息
  vtkSmartPointer<vtkDataArray> normal_data = mesh_data_->GetPointData()->GetNormals();

  // 如果网格数据没有法线，返回 false
  if (!normal_data)
  {
    return false;
  }

  // 默认法线向量为 (0, 0, 1)（单位z轴方向）
  Eigen::Vector3d normal_vect = Eigen::Vector3d::UnitZ();

  // 遍历每一个点，在输入数据 (data) 中为每个点计算法线
  for (int i = 0; i < data->GetPoints()->GetNumberOfPoints(); ++i)
  {
    // 获取当前点的坐标
    Eigen::Vector3d query_point;
    vtkSmartPointer<vtkIdList> id_list = vtkSmartPointer<vtkIdList>::New();
    data->GetPoints()->GetPoint(i, query_point.data());  // 获取当前点的坐标
    // 在 kd-tree 中查找距离当前点一定半径范围内的所有点
    kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list);

    // 如果没有找到点，则查找最近的一个点
    if (id_list->GetNumberOfIds() < 1)
    {
      kd_tree_->FindClosestNPoints(1, query_point.data(), id_list);

      // 如果还是没有找到点，则返回 false
      if (id_list->GetNumberOfIds() < 1)
      {
        return false;
      }
    }

    // 计算邻近点法线的平均值
    normal_vect = Eigen::Vector3d::Zero();  // 初始化法线向量为零向量
    std::size_t num_normals = 0;  // 记录参与平均的法线数量

    // 遍历在 kd-tree 中找到的所有点，计算法线的平均值
    for (auto p = 0; p < id_list->GetNumberOfIds(); p++)
    {
      Eigen::Vector3d temp_normal;  // 临时存储每个点的法线
      vtkIdType p_id = id_list->GetId(p);  // 获取当前点的 ID

      // 如果点 ID 无效，跳过该点
      if (p_id < 0)
      {
        continue;
      }

      // 获取当前点的法线并累加
      normal_data->GetTuple(p_id, temp_normal.data());  // 获取该点的法线数据
      normal_vect += temp_normal.normalized();  // 将法线向量归一化后累加
      num_normals++;  // 增加法线计数
    }

    // 如果有法线，计算平均法线并归一化
    if (num_normals > 0)
    {
      normal_vect /= num_normals;  // 计算平均法线
      normal_vect.normalize();  // 归一化平均法线
    }

    // 将计算得到的法线设置到新的法线数组中
    new_normals->SetTuple3(i, normal_vect(0), normal_vect(1), normal_vect(2));  // 将法线保存到 new_normals 数组中
  }

  // 将计算的法线数据设置到输入数据的点数据中
  data->GetPointData()->SetNormals(new_normals);  // 将新的法线数组设置到数据中
  return true;  // 返回成功
}
}  // namespace

namespace noether
{
PlaneSlicerRasterPlanner::PlaneSlicerRasterPlanner(DirectionGenerator::ConstPtr dir_gen,
                                                   OriginGenerator::ConstPtr origin_gen)
  : RasterPlanner(std::move(dir_gen), std::move(origin_gen))
{
}

void PlaneSlicerRasterPlanner::setMinSegmentSize(const double min_segment_size)
{
  min_segment_size_ = min_segment_size;
}
void PlaneSlicerRasterPlanner::setSearchRadius(const double search_radius) { search_radius_ = search_radius; }

ToolPaths PlaneSlicerRasterPlanner::planImpl(const pcl::PolygonMesh& mesh) const
{
  // 将输入的PCL网格转换为VTK类型
  vtkSmartPointer<vtkPolyData> mesh_data_ = vtkSmartPointer<vtkPolyData>::New();
  pcl::VTKUtils::mesh2vtk(mesh, mesh_data_);  // 将PCL网格转换为vtkPolyData格式
  mesh_data_->BuildLinks();  // 构建点和单元之间的链接（用于高效访问）
  mesh_data_->BuildCells();  // 构建网格的单元数据结构
  bool hasPointNormalsFlag = mesh_data_->GetPointData()->GetNormals();
  bool hasCellNormalsFlag = mesh_data_->GetCellData()->GetNormals();
  // 检查网格数据是否有法线信息，如果没有则计算法线
  if (!has_normals_flag|| !hasCellNormalsFlag)
  {
    // 创建法线生成器
    vtkSmartPointer<vtkPolyDataNormals> normal_generator = vtkSmartPointer<vtkPolyDataNormals>::New();
    normal_generator->SetInputData(mesh_data_);  // 设置输入数据
    normal_generator->ComputePointNormalsOn();  // 开启点法线计算
    normal_generator->SetComputeCellNormals(!mesh_data_->GetCellData()->GetNormals());  // 如果没有单元法线则计算
    normal_generator->SetFeatureAngle(M_PI_2);  // 设置特征角度阈值
    normal_generator->SetSplitting(true);  // 开启法线拆分
    normal_generator->SetConsistency(true);  // 启用一致性法线
    normal_generator->SetAutoOrientNormals(false);  // 不自动调整法线方向
    normal_generator->SetFlipNormals(false);  // 不翻转法线
    normal_generator->SetNonManifoldTraversal(false);  // 不进行非流形遍历
    normal_generator->Update();  // 计算法线

    // 如果没有点法线，将计算的法线设置到点数据中
    if (!mesh_data_->GetPointData()->GetNormals())
    {
      mesh_data_->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals());
    }

    // 如果没有单元法线，将计算的法线设置到单元数据中
    if (!mesh_data_->GetCellData()->GetNormals())
    {
      mesh_data_->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals());
    }
  }

  // 使用主成分分析（PCA）来确定网格的主要轴方向
  Eigen::Vector3d mesh_normal;  // 网格法线，单位向量
  Eigen::Matrix3d pca_vecs;     // PCA计算得到的主成分向量，按网格的最大尺度缩放
  Eigen::Vector3d centroid;     // 网格的质心
  {
    // 将PCL点云转换为Eigen格式
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromPCLPointCloud2(mesh.cloud, *cloud);  // 将PCL网格的点云转换为PCL点云对象

    // 执行PCA分析
    pcl::PCA<pcl::PointXYZ> pca;
    pca.setInputCloud(cloud);  // 设置输入点云

    // 投影点云数据到主成分空间
    pcl::PointCloud<pcl::PointXYZ> proj;
    pca.project(*cloud, proj);  // 执行点云投影
    pcl::PointXYZ min, max;
    pcl::getMinMax3D(proj, min, max);  // 获取投影结果的最小最大坐标
    Eigen::Array3f scales = max.getArray3fMap() - min.getArray3fMap();  // 计算各主成分的尺度

    // 提取PCA结果中的第三个主成分作为网格法线，并归一化
    mesh_normal = pca.getEigenVectors().col(2).cast<double>().normalized();
    centroid = pca.getMean().head<3>().cast<double>();  // 获取点云的质心
    pca_vecs = (pca.getEigenVectors().array().rowwise() * scales.transpose()).cast<double>();  // 对PCA向量进行缩放
  }

  // 获取初始的切割平面方向
  Eigen::Vector3d cut_direction = dir_gen_->generate(mesh);  // 生成切割方向
  // 计算切割平面的法线，切割方向与网格法线的叉积
  Eigen::Vector3d cut_normal = (cut_direction.normalized().cross(mesh_normal)).normalized();

  // 计算切割网格所需的平面数量，根据主成分的尺度来确定
  double max_extent =
      std::sqrt(pca_vecs.col(0).squaredNorm() + pca_vecs.col(1).squaredNorm() + pca_vecs.col(2).squaredNorm());  // 计算主成分方向的最大长度
  std::size_t num_planes = static_cast<std::size_t>(max_extent / line_spacing_);  // 根据主成分的尺度和每个平面的间隔计算平面数量

  // 计算切割的起始位置，质心减去网格的半对角线（网格的边界框的一半）
  Eigen::Vector3d start_loc = centroid - (max_extent / 2.0) * cut_normal;

  // 创建一个vtkAppendPolyData对象，用于收集所有的切割数据
  vtkSmartPointer<vtkAppendPolyData> raster_data = vtkSmartPointer<vtkAppendPolyData>::New();
  
  // 生成每一个切割平面并收集交点数据
  for (std::size_t i = 0; i < num_planes + 1; i++)
  {
    // 创建切割平面、切割器和条带生成器
    vtkSmartPointer<vtkPlane> plane = vtkSmartPointer<vtkPlane>::New();  // 创建切割平面对象
    vtkSmartPointer<vtkCutter> cutter = vtkSmartPointer<vtkCutter>::New();  // 创建切割器
    vtkSmartPointer<vtkStripper> stripper = vtkSmartPointer<vtkStripper>::New();  // 创建条带生成器

    // 计算当前切割平面的起始位置
    Eigen::Vector3d current_loc = start_loc + i * line_spacing_ * cut_normal;
    plane->SetOrigin(current_loc.x(), current_loc.y(), current_loc.z());  // 设置当前平面的起始点
    plane->SetNormal(cut_normal.x(), cut_normal.y(), cut_normal.z());  // 设置当前平面的法线

    cutter->SetCutFunction(plane);  // 将切割平面传递给切割器
    cutter->SetInputData(mesh_data_);  // 设置输入的网格数据
    cutter->SetSortBy(1);  // 设置切割结果排序方式
    cutter->SetGenerateTriangles(false);  // 禁止生成三角形数据
    cutter->Update();  // 执行切割操作

    // 设置条带生成器的输入并执行条带生成操作
    stripper->SetInputConnection(cutter->GetOutputPort());  // 设置条带生成器的输入
    stripper->JoinContiguousSegmentsOn();  // 启用连接连续线段的功能
    stripper->SetMaximumLength(mesh_data_->GetNumberOfPoints());  // 设置最大线段长度
    stripper->Update();  // 执行条带操作

    // 如果发生错误，跳过当前切割
    if (stripper->GetErrorCode() != vtkErrorCode::NoError)
    {
      continue;  // 如果条带生成失败，则跳过
    }

    // 将切割结果添加到raster_data中
    for (int r = 0; r < stripper->GetNumberOfOutputPorts(); r++)
    {
      raster_data->AddInputData(stripper->GetOutput(r));  // 将生成的结果添加到集合中
    }
  }

  // 创建KD树用于查找点，并建立单元查找器
  vtkSmartPointer<vtkKdTreePointLocator> kd_tree_ = vtkSmartPointer<vtkKdTreePointLocator>::New();  // 创建一个点查找器
  kd_tree_->SetDataSet(mesh_data_);  // 设置数据集
  kd_tree_->BuildLocator();  // 构建KD树

  vtkSmartPointer<vtkCellLocator> cell_locator_ = vtkSmartPointer<vtkCellLocator>::New();  // 创建单元查找器
  cell_locator_->SetDataSet(mesh_data_);  // 设置数据集
  cell_locator_->BuildLocator();  // 构建单元查找器

  // collect rasters and set direction
  raster_data->Update();  // 更新 raster_data，使其准备好获取输入数据。
  vtkIdType num_slices = raster_data->GetTotalNumberOfInputConnections();  // 获取 raster_data 的输入连接数（即 slices 的数量）。
  std::vector<RasterConstructData> rasters_data_vec;  // 创建一个向量，用来存储 raster 数据。
  std::vector<std::vector<vtkIdType>> raster_ids;  // 存储每个 raster 切片中的点的 ID。
  for (std::size_t i = 0; i < num_slices; i++)  // 遍历每个 slice。
  {
      RasterConstructData r;  // 创建一个 RasterConstructData 对象，用来存储当前 slice 的结果。

      // collecting raster segments based on min hole size
      vtkSmartPointer<vtkPolyData> raster_lines = raster_data->GetInput(i);  // 获取第 i 个切片的 PolyData（即包含所有线段的几何数据）。
      vtkIdType* indices;  // 存储每个线段的点 ID 的指针。
      vtkIdType num_points;  // 存储每个线段的点数。
      vtkIdType num_lines = raster_lines->GetNumberOfLines();  // 获取该切片中线段的数量。
      vtkCellArray* cells = raster_lines->GetLines();  // 获取切片中所有线段的单元格数据。

      if (num_lines == 0)  // 如果当前切片没有任何线段，跳过该切片。
      {
        continue;
      }

      raster_ids.clear();  // 清空 raster_ids 向量，用于存储当前切片中的点 ID。

      unsigned int lineCount = 0;  // 用于计数线段。
      for (cells->InitTraversal(); cells->GetNextCell(num_points, indices); lineCount++)  // 遍历每一条线段。
      {
        std::vector<vtkIdType> point_ids;  // 用于存储当前线段的点 ID。

        for (vtkIdType i = 0; i < num_points; i++)  // 遍历当前线段的每个点。
        {
          if (std::find(point_ids.begin(), point_ids.end(), indices[i]) != point_ids.end())  // 检查当前点 ID 是否已经在 point_ids 中。
          {
            continue;  // 如果已经存在，跳过该点。
          }
          point_ids.push_back(indices[i]);  // 如果该点还没有出现过，则将其添加到 point_ids 中。
        }

        if (point_ids.empty())  // 如果没有有效的点，跳过。
        {
          continue;
        }

        // removing duplicates
        auto iter = std::unique(point_ids.begin(), point_ids.end());  // 移除重复的点 ID。
        point_ids.erase(iter, point_ids.end());  // 删除重复的点。

        // adding
        raster_ids.push_back(point_ids);  // 将当前线段的点 ID 存入 raster_ids。
      }

      if (raster_ids.empty())  // 如果没有有效的 raster IDs，跳过当前切片。
      {
        continue;
      }

      // remove redundant indices
      removeRedundant(raster_ids);  // 移除冗余的点 ID，确保每个 raster 都是唯一的。

      // merging segments
      mergeRasterSegments(raster_lines->GetPoints(), min_hole_size_, raster_ids);  // 根据最小孔隙尺寸合并 raster 中的线段。

      // rectifying
      if (rasters_data_vec.size() > 1)  // 如果已有多个 raster 数据，则进行方向校正。
      {
        Eigen::Vector3d ref_point;
        rasters_data_vec.back().raster_segments.front()->GetPoint(0, ref_point.data());  // 获取前一个 raster 的第一个点作为参考点。
        rectifyDirection(raster_lines->GetPoints(), ref_point, raster_ids);  // 校正当前切片的方向。
      }

      for (auto& rpoint_ids : raster_ids)  // 遍历所有的点 ID 列表（每个列表代表一个线段）。
      {
        // Populating with points
        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();  // 创建一个新的 vtkPoints 对象，用来存储当前线段的点。
        std::for_each(rpoint_ids.begin(), rpoint_ids.end(), [&points, &raster_lines](vtkIdType& id) {
          std::array<double, 3> p;
          raster_lines->GetPoint(id, p.data());  // 获取当前点的坐标。
          points->InsertNextPoint(p.data());  // 将该点的坐标插入 points 中。
        });

        // compute length and add points if segment length is greater than threshold
        double line_length = computeLength(points);  // 计算当前线段的长度。
        if (line_length > min_segment_size_ && points->GetNumberOfPoints() > 1)  // 如果线段长度大于最小长度阈值，并且线段包含多个点。
        {
          // enforce point spacing
          vtkSmartPointer<vtkPoints> new_points = applyParametricSpline(points, line_length, point_spacing_);  // 通过样条曲线强制点之间的间隔。

          // add points to segment now
          vtkSmartPointer<vtkPolyData> segment_data = vtkSmartPointer<vtkPolyData>::New();  // 创建一个新的 PolyData 对象，用来存储该段线段的数据。
          segment_data->SetPoints(new_points);  // 设置该段线段的点数据。

          // inserting normals
          if (!insertNormals(search_radius_, mesh_data_, kd_tree_, segment_data))  // 插入法线信息（例如通过 k-d 树查找法线）。
          {
            throw std::runtime_error("Could not insert normals for segment " + std::to_string(r.raster_segments.size()) +
                                    " of raster " + std::to_string(i));  // 如果法线插入失败，抛出异常。
          }

          // saving into raster
          r.raster_segments.push_back(segment_data);  // 将当前处理的 segment 数据存入 raster_segments。
          r.segment_lengths.push_back(line_length);  // 将当前 segment 的长度存入 segment_lengths。
        }
      }

      // Save raster
      if (!r.raster_segments.empty())  // 如果当前 raster 有有效的 segments，则将其保存。
        rasters_data_vec.push_back(r);  // 将当前 raster 数据存入 rasters_data_vec。
    }

    // converting to poses msg now
    ToolPaths tool_paths = convertToPoses(rasters_data_vec);  // 将 raster 数据转换为工具路径的消息格式。
    return tool_paths;  // 返回工具路径。
  }

  }  // namespace noether
